unde.games.collision_detector source code

1 module unde.games.collision_detector;
2 
3 import std.algorithm.sorting;
4 import std.math;
5 import std.conv;
6 import std.string;
7 import std.stdio;
8 import std.algorithm.comparison;
9 
10 import unde.games.obj_loader;
11 enum Intersect
12 {
13     No = 0,
14     Almost = 1,
15     Yes = 2,
16     In = 3
17 }
18 
19 struct Vector
20 {
21     float x,y,z;
22 }
23 
24 void scene_to_collision_object (const (ObjFile) *sc, ref Vector[][string] output)
25 {
26     foreach (object; sc.objects)
27     {
28         uint i;
29         uint n = 0, t;
30         
31         string name = object.name;
32         Intersect intersect = Intersect.No;
33         
34         Vector[] vertices;
35         typeof(vertices) new_vertices;
36     
37         foreach (ref v; object.vertices)
38         {
39             vertices ~= Vector(-v[0], v[1], v[2]);
40         }
41     
42         //vertices.sort!("a.x < b.x || a.x==b.x && (a.y < b.y || a.y==b.y && a.z < b.z)");
43         vertices.sort!("a.x > b.x + 0.001 || abs(a.x-b.x) < 0.001 && (a.y > b.y + 0.001 || abs(a.y-b.y) < 0.001 && a.z > b.z + 0.001)");
44     
45         for (t = 1; t < vertices.length; t++)
46         {
47             if (!(abs(vertices[t-1].x-vertices[t].x) < 0.001 && abs(vertices[t-1].y-vertices[t].y) < 0.001 &&
48                   abs(vertices[t-1].z-vertices[t].z) < 0.001))
49             {
50                 new_vertices ~= Vector(
51                         vertices[t-1].x,
52                         vertices[t-1].y,
53                         vertices[t-1].z);
54             }
55         }
56     
57         new_vertices ~= Vector(
58                         vertices[$-1].x,
59                         vertices[$-1].y,
60                         vertices[$-1].z);
61         vertices = new_vertices;
62     
63         bool _join;
64         do
65         {
66             new_vertices = null;
67             _join = false;
68             if (name.startsWith("GroundGarden")) writefln("vertices.length = %s", vertices.length);
69             for (t = 1; t < vertices.length; t++)
70             {
71                 if (abs(vertices[t-1].x-vertices[t].x) < 0.001 && abs(vertices[t-1].y-vertices[t].y) < 0.001)
72                 {
73                     if (name.startsWith("GroundGarden")) writefln("x:%s,%s, y=%s,%s z:(%s+%s)/2=%s",
74                         vertices[t-1].x, vertices[t].x,
75                         vertices[t-1].y, vertices[t].y,
76                         vertices[t-1].z, vertices[t].z, (vertices[t-1].z+vertices[t].z)/2);
77                     new_vertices ~= Vector(
78                         (vertices[t-1].x+vertices[t].x)/2,
79                         (vertices[t-1].y+vertices[t].y)/2,
80                         (vertices[t-1].z+vertices[t].z)/2);
81                     _join = true;
82                     t++;
83                 }
84                 else new_vertices ~= vertices[t-1];
85                 
86                 if ( t+1 == vertices.length )
87                     new_vertices ~= vertices[t];
88             }
89             vertices = new_vertices;
90         } while (_join);
91     
92         for (t = 1; t < new_vertices.length; t++)
93         {
94             if (abs(new_vertices[t-1].y - new_vertices[t].y) < 0.001)
95             {
96                 float y = (new_vertices[t-1].y + new_vertices[t].y)/2;
97                 new_vertices[t-1].y = y;
98                 new_vertices[t].y = y;
99                 t++;
100             }
101         }
102     
103         for (t = 1; t < new_vertices.length; t++)
104         {
105             if (abs(new_vertices[t-1].x - new_vertices[t].x) < 0.001)
106             {
107                 float x = (new_vertices[t-1].x + new_vertices[t].x)/2;
108                 new_vertices[t-1].x = x;
109                 new_vertices[t].x = x;
110                 t++;
111             }
112         }
113     
114         output[name] = new_vertices;
115     }
116 }
117 
118 float vecLen(float[2] v)
119 {
120     return sqrt(v[0]^^2 + v[1]^^2);
121 }
122 
123 Intersect if_intersect (float[2] p1, float[2] p2, float[2] cp, float[2] p, bool _debug=false)
124 {
125     float[2] P2 = p2[] - p1[];
126     float[2] CP = cp[] - p[];
127 
128     float x, y;
129 
130     if (abs(P2[0]) < 0.001)
131     {
132         if (_debug) writefln("P2=%s, CP=%s", P2, CP);
133         x = p1[0];
134         if (CP[1] == 0.0)
135         {
136             y = cp[1];
137         }
138         else
139         {
140             y = (x - p[0])*CP[1]/CP[0] + p[1];
141         }
142     }
143     else if (abs(CP[1]) < 0.001)
144     {
145         if (_debug) writefln("P2=%s, CP=%s", P2, CP);
146         y = p[1];
147         x = (y - p1[1])*P2[0]/P2[1] + p1[0];
148     }
149     else
150     {
151         x = (p[0]/CP[0] - p1[0]*P2[1]/P2[0]/CP[1] + (p1[1] - p[1])/CP[1]) / (1/CP[0] - P2[1]/P2[0]/CP[1]);
152         y = (x - p1[0])*P2[1]/P2[0] + p1[1];
153     }
154 
155     float[2] cross = [x, y];
156     float[2] cr = p[] - cross[];
157     float[2] cd = cp[] - cross[];
158 
159     float lambda = vecLen(cr)/vecLen(cd);
160 
161     foreach (ref pp; cr)
162     {
163         if (abs(pp) < 0.01) pp = 0.0;
164     }
165 
166     for (int i; i < 2; i++)
167     {
168         if (cr[i] == 0.0 && abs(cd[i]) < 0.1) cd[i] = 0.0;
169     }
170 
171     if ( !(sgn(cr[0]) != sgn(cd[0]) &&
172         sgn(cr[1]) != sgn(cd[1])) &&
173         (sgn(cr[0]) != sgn(cd[0]) ||
174         sgn(cr[1]) != sgn(cd[1])) &&
175         cr[0] != 0.0 && cr[1] != 0.0 &&
176         cd[0] != 0.0 && cd[1] != 0.0)
177     {
178         if(_debug)
179         { 
180             writefln("%s!=%s, %s!=%s",
181                 sgn(cr[0]), sgn(cd[0]),
182                 sgn(cr[1]), sgn(cd[1]));
183             writefln("cr=%s, cd=%s", cr, cd);
184         }
185         if (_debug) writefln("OH %s, %s, %s, %s: cross=%s, lambda = %s", p1, p2, cp, p, cross, lambda);
186         return Intersect.Yes;
187     }
188 
189     if (sgn(cr[0]) != sgn(cd[0]) && cr[0] != 0.0 && cd[0] != 0.0 ||
190         sgn(cr[1]) != sgn(cd[1]) && cr[1] != 0.0 && cd[1] != 0.0) lambda = -lambda;
191 
192     if (_debug) writefln("%s, %s, %s, %s: cross=%s, lambda = %s (cr=%s, cd=%s)", p1, p2, cp, p, cross, lambda, cr, cd);
193     
194     if (lambda > 0.0) return Intersect.In;
195     else if (lambda == 0.0) return Intersect.Yes;
196     else return Intersect.No;
197 }
198 
199 long if_intersect (float[2] p1, float[2] p2, float[2] cp, float[4] box, bool _debug=false)
200 {
201     assert(p1 != p2);
202     assert(p1 != cp);
203     assert(p2 != cp);
204     
205     float[2][] points;
206     points ~= [ box[0], box[1] ];
207     points ~= [ box[0], box[3] ];
208     points ~= [ box[2], box[1] ];
209     points ~= [ box[2], box[3] ];
210 
211     Intersect max_i = Intersect.No;
212     Intersect min_i = Intersect.In;
213 
214     long inners;
215 
216     foreach(j, p; points)
217     {
218         auto i = if_intersect (p1, p2, cp, p, _debug);
219         if (i == Intersect.In)
220             inners |= 2^^j;
221         max_i = max(max_i, i);
222         min_i = min(min_i, i);
223     }
224 
225     if (_debug) writefln("Отрезок, точка и бокс. (%s, %s) %s, %s, %s, %s",
226         min_i, max_i, p1, p2, cp, box);
227 
228     if (max_i == min_i) return min_i;
229 
230     float[2] P2 = p2[] - p1[];
231     float x, y;
232     x = box[0];
233     if (p1[0] < x && x < p2[0] ||
234         p2[0] < x && x < p1[0])
235     {
236         y = (x - p1[0])*P2[1]/P2[0] + p1[1];
237         if (_debug) writefln("1. (%s, %s)", x, y);
238         if (box[1] < y && y < box[3])
239             return Intersect.Yes;
240     }
241 
242     x = box[2];
243     if (p1[0] < x && x < p2[0] ||
244         p2[0] < x && x < p1[0])
245     {
246         y = (x - p1[0])*P2[1]/P2[0] + p1[1];
247         if (_debug) writefln("2. (%s, %s)", x, y);
248         if (box[1] < y && y < box[3])
249             return Intersect.Yes;    
250     }
251 
252     y = box[1];
253     if (p1[1] < y && y < p2[1] ||
254         p2[1] < y && y < p1[1])
255     {
256         x = (y - p1[1])*P2[0]/P2[1] + p1[0];
257         if (_debug) writefln("3. (%s, %s)", x, y);
258         if (box[0] < x && x < box[2])
259             return Intersect.Yes;
260     }
261 
262     y = box[3];
263     if (p1[1] < y && y < p2[1] ||
264         p2[1] < y && y < p1[1])
265     {
266         x = (y - p1[1])*P2[0]/P2[1] + p1[0];
267         if (_debug) writefln("4. (%s, %s)", x, y);
268         if (box[0] < x && x < box[2])
269             return Intersect.Yes;
270     }
271 
272     if (box[0] < p1[0] && p1[0] < box[2] &&
273         box[1] < p1[1] && p1[1] < box[3] ||
274         box[0] < p2[0] && p2[0] < box[2] &&
275         box[1] < p2[1] && p2[1] < box[3])
276         return Intersect.Yes;
277 
278     if (_debug) writefln("no");
279     return -inners;
280 }
281 
282 string last_intersect;
283 
284 struct CollisionCacheEntry
285 {
286     string name;
287     size_t from;
288     size_t to;
289 }
290 
291 struct SC
292 {
293     int x, y;
294 }
295 
296 private CollisionCacheEntry[][SC][void *] collision_cache;
297 
298 void reset_collision_cache()
299 {
300     collision_cache = null;
301 }
302 
303 Intersect if_intersect (Vector[][string] co, float[6] box, bool _debug = false)
304 in
305 {
306     assert(box[0] < box[3], format("Box x0=%s must be less x1=%s", box[0], box[3]));
307     assert(box[1] < box[4]);
308     assert(box[2] < box[5]);
309 }
310 body
311 {
312     size_t j;
313 
314     Intersect intersect = Intersect.No;
315 
316     Intersect check_object(string name, Vector[] object, size_t from, size_t to,
317         void *co, SC *sc)
318     {
319         Intersect intersect = Intersect.No;
320 
321         size_t cfrom = object.length, cto = min(2, object.length);
322 
323         if (object.length < 3) return intersect;
324 
325         /*if (abs(object[0].y - box[1]) > 3.0 &&
326             abs(object[$-1].y - box[1]) > 3.0 &&
327             abs(object[$-1].y - object[0].y) < 3.0)
328             return intersect;*/
329         
330         for (j = from; j < to; j++)
331         {
332             auto v1 = object[j-2];
333             auto v2 = object[j-1];
334             auto v3 = object[j];
335             typeof(v3) v4;
336             if (j+1 < object.length) v4 = object[j+1];
337             
338             bool __debug = _debug;
339 
340             if (co)
341             {
342                 auto vl = j+1 < object.length?v4:v3;
343                 if (v1.x < (sc.x*30 - 20) || vl.x > (sc.x*30 + 20) ||
344                     v1.y < (sc.y*17 - 10) && vl.y < (sc.y*17 - 10) || 
345                     v1.y > (sc.y*17 + 10) && vl.y > (sc.y*17 + 10))
346                 {
347                     continue;
348                 }
349             
350                 if (j < cfrom) cfrom = j;
351                 if (j+1 > cto) cto = j+1;
352             }
353             
354             if (_debug && abs(v1.x - box[0]) > 3.0 && abs(v3.x - box[0]) > 3.0) __debug = false;
355             if (abs(v1.x - box[0]) > 3.0 && abs(v3.x - box[0]) > 3.0 &&
356                 abs(v3.x - v1.x) < 3.0) continue;
357 
358             if (v1.x == v2.x && v3.x == v4.x)
359             {
360                 float z = (v1.z + v2.z + v3.z + v4.z)/4;
361                 if (box[2] <= z && z <= box[5])
362                 {
363                     Intersect max_i = Intersect.No;
364                     Intersect min_i = Intersect.In;
365                     long inners = 2^^4-1;
366                     
367                     auto i = if_intersect ([v1.x, v1.y], [v2.x, v2.y], [v3.x, v3.y],
368                         [box[0], box[1], box[3], box[4]], __debug);
369                     Intersect inter;
370                     inter = Intersect.No;
371                     if (i > 0) inter = cast(Intersect) i;
372                     if (i <= 0) inners &= -i;
373                     max_i = max(max_i, inter);
374                     min_i = min(min_i, inter);
375     
376                     i = if_intersect ([v1.x, v1.y], [v3.x, v3.y], [v4.x, v4.y],
377                         [box[0], box[1], box[3], box[4]], __debug);
378                     inter = Intersect.No;
379                     if (i > 0) inter = cast(Intersect) i;
380                     if (i <= 0) inners &= -i;
381                     max_i = max(max_i, inter);
382                     min_i = min(min_i, inter);
383     
384                     i = if_intersect ([v2.x, v2.y], [v4.x, v4.y], [v1.x, v1.y],
385                         [box[0], box[1], box[3], box[4]], __debug);
386                     inter = Intersect.No;
387                     if (i > 0) inter = cast(Intersect) i;
388                     if (i <= 0) inners &= -i;
389                     max_i = max(max_i, inter);
390                     min_i = min(min_i, inter);
391     
392                     i = if_intersect ([v3.x, v3.y], [v4.x, v4.y], [v1.x, v1.y],
393                         [box[0], box[1], box[3], box[4]], __debug);
394                     inter = Intersect.No;
395                     if (i > 0) inter = cast(Intersect) i;
396                     if (i <= 0) inners &= -i;
397                     max_i = max(max_i, inter);
398                     min_i = min(min_i, inter);
399 
400                     if (__debug) writefln("%s. (%s, %s, %s) points: %s, %s, %s, %s, box: %s", name, 
401                         min_i, max_i, inners, v1, v2, v3, v4, box);
402     
403                     if (min_i > Intersect.No || inners > 0)
404                     {
405                         last_intersect = name;
406                         
407                         if (_debug) writefln("%s. %s", min_i, name);
408                         if (!co) return max(min_i, Intersect.Yes);
409                         else intersect = max(min_i, Intersect.Yes, intersect);
410                     }
411                 }
412                     
413                 j++;
414             }
415             else
416             {
417                 float z = (v1.z + v2.z + v3.z)/4;
418                 if (box[2] <= z && z <= box[5])
419                 {
420                     Intersect max_i = Intersect.No;
421                     Intersect min_i = Intersect.In;
422                     long inners = 2^^4-1;
423                     
424                     auto i = if_intersect ([v1.x, v1.y], [v2.x, v2.y], [v3.x, v3.y],
425                         [box[0], box[1], box[3], box[4]], __debug);
426                         
427                     Intersect inter;
428                     inter = Intersect.No;
429                     if (i > 0) inter = cast(Intersect) i;
430                     if (i <= 0) inners &= -i;
431                     max_i = max(max_i, inter);
432                     min_i = min(min_i, inter);
433     
434                     i = if_intersect ([v2.x, v2.y], [v3.x, v3.y], [v1.x, v1.y],
435                         [box[0], box[1], box[3], box[4]], __debug);
436                     inter = Intersect.No;
437                     if (i > 0) inter = cast(Intersect) i;
438                     if (i <= 0) inners &= -i;
439                     max_i = max(max_i, inter);
440                     min_i = min(min_i, inter);
441     
442                     i = if_intersect ([v3.x, v3.y], [v1.x, v1.y], [v2.x, v2.y],
443                         [box[0], box[1], box[3], box[4]], __debug);
444                     inter = Intersect.No;
445                     if (i > 0) inter = cast(Intersect) i;
446                     if (i <= 0) inners &= -i;
447                     max_i = max(max_i, inter);
448                     min_i = min(min_i, inter);
449 
450                     if (__debug) writefln("%s. (%s, %s, %s) points: %s, %s, %s, box: %s", name, 
451                         min_i, max_i, inners, v1, v2, v3, box);
452     
453                     if (min_i > Intersect.No || inners > 0)
454                     {
455                         last_intersect = name;
456                         if (_debug) writefln("%s. %s", min_i, name);
457                         if (!co) return max(min_i, Intersect.Yes);
458                         else intersect = max(min_i, Intersect.Yes, intersect);
459                     }
460                 }
461             }
462         }
463 
464         if (co && cto > cfrom)
465         {
466             collision_cache[co][*sc] ~= CollisionCacheEntry(name, cfrom, cto);
467         }
468         
469         return intersect;
470     }
471 
472     float cx = (box[0] + box[3])/2;
473     float cy = (box[1] + box[4])/2;
474 
475     SC sc = SC(cast(int)round(cx/30), cast(int)round(cy/17));
476 
477     if (cast(void*) co !in collision_cache ||
478         sc !in collision_cache[cast(void*) co])
479     {
480         if (cast(void*)co !in collision_cache)
481             collision_cache[cast(void*)co] = null;
482             
483         if (sc !in collision_cache[cast(void*)co])
484             collision_cache[cast(void*)co][sc] = [];
485 
486         foreach (name, object; co)
487         {
488             //writefln("%s from %s to %s", name, 2, object.length);
489             intersect = max(intersect, check_object(name, object, 2, object.length, cast(void*) co, &sc));
490         }
491     }
492     else
493     {
494         //writefln("%s objects instead of %s", collision_cache[cast(void*) co][sc].length, co.length);
495         foreach (cache_entry; collision_cache[cast(void*) co][sc])
496         {
497             //writefln("*%s from %s to %s", cache_entry.name, cache_entry.from, cache_entry.to);
498             intersect = check_object(cache_entry.name, co[cache_entry.name],
499                 cache_entry.from, cache_entry.to, null, &sc);
500             if (intersect > Intersect.No) return intersect;
501         }
502     }
503             
504     return intersect;    
505 }
506